#include <pcl/registration/inputGen.h>


// void GenIn::corrComp(pcl::PointCloud<pcl::MultiScaleF864>::Ptr fpfhs,
//                                    pcl::PointCloud<pcl::MultiScaleF864>::Ptr fpfht,
//                                    vector<corrTab> &corr, int MaxNOCorrPP,
//                                    vector<int> &corrNOS, vector<int> &corrNOT){
//     int n = min(MaxNOCorrPP, (int)fpfht->size()); //maximum number of correspondences to find for each source point
//     corr.clear();
//     corrNOS.assign(fpfhs->size(),0);
//     corrNOT.assign(fpfht->size(),0);
//     // Use a KdTree to search for the nearest matches in feature space
//     pcl::KdTreeFLANN<pcl::MultiScaleF864> treeS;
//     treeS.setInputCloud (fpfhs);
//     pcl::KdTreeFLANN<pcl::MultiScaleF864> treeT;
//     treeT.setInputCloud (fpfht);
//     for(size_t i=0;i<fpfhs->size();i++){
//         vector<int> corrIdxTmp(n);
//         vector<float> corrDisTmp(n);
//         //find the best n matches in target fpfh
//         treeT.nearestKSearch(*fpfhs,i,n,corrIdxTmp,corrDisTmp);
//         for(size_t j=0;j<corrIdxTmp.size();j++){
//             bool removeFlag = true;
//             int searchIdx = corrIdxTmp[j];
//             vector<int> corrIdxTmpT(n);
//             vector<float> corrDisTmpT(n);
//             treeS.nearestKSearch(*fpfht,searchIdx,n,corrIdxTmpT,corrDisTmpT);
//             for(size_t k=0;k<n;k++){
//                 if(corrIdxTmpT.data()[k]==i){
//                     removeFlag = false;
//                     break;
//                 }
//             }
//             if(removeFlag == false){
//                 corrTab corrTabTmp;
//                 corrTabTmp.idxS = i;
//                 corrTabTmp.idxT = corrIdxTmp[j];
//                 corrTabTmp.disFeature = corrDisTmp[j];
//                 corrTabTmp.upBnd = fpfht->size();
//                 corrTabTmp.lwBnd = 1;
//                 corr.push_back(corrTabTmp);
//                 corrNOS[i]++;
//                 corrNOT[corrIdxTmp[j]]++;
//             }
//         }
//     }
// }
void GenIn::VGF(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr cloudVG,
                double inlTh){
    //format for filtering
    pcl::PCLPointCloud2::Ptr cloud2 (new pcl::PCLPointCloud2 ());
    pcl::PCLPointCloud2::Ptr cloudVG2 (new pcl::PCLPointCloud2 ());
    pcl::toPCLPointCloud2(*cloud,*cloud2);
    //set up filtering parameters
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud (cloud2);
    sor.setLeafSize (inlTh, inlTh, inlTh);
    //filtering process
    sor.filter (*cloudVG2);
    pcl::fromPCLPointCloud2(*cloudVG2,*cloudVG);
}

void GenIn::ISSExt(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,pcl::PointIndicesPtr pointsIdx,pcl::PointIndicesPtr ISSIdx, double inlTh){
    //product the input point cloud
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(cloud);
	extract.setIndices(pointsIdx);
	//extract.setNegative(true);	
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_indice(new pcl::PointCloud<pcl::PointXYZ>);
	extract.filter(*cloud_indice);
    //calculate the ISS keypoints
    double iss_salient_radius_ = 6 * inlTh;
    double iss_non_max_radius_ = 4 * inlTh;
    double iss_gamma_21_ (0.975);
    double iss_gamma_32_ (0.975);
    double iss_min_neighbors_ (5);
    int iss_threads_ (4); //switch to the number of threads in your cpu for acceleration
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    //tree->setInputCloud(cloud);
    pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss_detector;
    iss_detector.setSearchMethod (tree);
    iss_detector.setSalientRadius (iss_salient_radius_);
    iss_detector.setNonMaxRadius (iss_non_max_radius_);
    iss_detector.setThreshold21 (iss_gamma_21_);
    iss_detector.setThreshold32 (iss_gamma_32_);
    iss_detector.setMinNeighbors (iss_min_neighbors_);
    iss_detector.setNumberOfThreads (iss_threads_);
    iss_detector.setInputCloud (cloud_indice);
    pcl::PointCloud<pcl::PointXYZ>::Ptr ISS(new pcl::PointCloud<pcl::PointXYZ>());
    iss_detector.compute (*ISS);
    if(ISS->points.size()==0){
        ISSIdx->indices=pointsIdx->indices;
        ISSIdx->header=pointsIdx->header;
    }
    else{
         //save the ISS point cloud
        pcl::PCDWriter writer;
        writer.write<pcl::PointXYZ> ("ISS1.pcd", *ISS, false);
        std::vector<int> global_index;
        std::vector<int> local_index=iss_detector.getKeypointsIndices()->indices;
        for(int i=0;i<local_index.size();++i){
            global_index.push_back(pointsIdx->indices[local_index[i]]);
        }
        ISSIdx->indices=global_index;
    }
   
}

void GenIn::FPFHComp(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud, pcl::PointCloud<pcl::Normal>::Ptr normals,double inlTh, pcl::PointIndicesPtr ISSIdx, pcl::PointCloud<pcl::FPFHSignature33>& fpfhOut){
    // //compute normal
     //pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal> ());
     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    // pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> neSource;
    // neSource.setInputCloud (cloud);
    // neSource.setSearchMethod (tree);
    // neSource.setRadiusSearch (3*inlTh);
    // neSource.compute (*normal);

    //compute fpfh using normals
    pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfhEst;
    fpfhEst.setInputCloud(cloud);
    fpfhEst.setInputNormals(normals);
    fpfhEst.setSearchMethod(tree);
    fpfhEst.setRadiusSearch(8*inlTh);
    //fpfhEst.setNumberOfThreads (4);
    fpfhEst.setIndices(ISSIdx);
    fpfhEst.compute(fpfhOut);
    cout<<fpfhOut.size()<<endl;
}

void GenIn::spinImageComp(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,pcl::PointCloud<pcl::Normal>::Ptr normals,pcl::PointIndicesPtr ISSIdx, double radius,pcl::PointCloud<pcl::Histogram<153> >& spinImageOut){
    pcl::SpinImageEstimation<pcl::PointXYZ,pcl::Normal,pcl::Histogram<153> >spinImage(8,0.5,5);//image with, support_angle_cos, min_pts_neighb
    
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    spinImage.setInputCloud(cloud);
    spinImage.setInputNormals(normals);
    spinImage.setSearchMethod(tree);
    spinImage.setRadiusSearch(radius*10);
    spinImage.setIndices(ISSIdx);
    spinImage.compute(spinImageOut);
    cout<<spinImageOut.size();

}
void GenIn::threeDSC(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud, pcl::PointCloud<pcl::Normal>::Ptr normals,pcl::PointIndicesPtr ISSIdx,double radius,pcl::PointCloud<pcl::ShapeContext1980>& threeSCOut){
  // Setup the shape context computation
  pcl::ShapeContext3DEstimation<pcl::PointXYZ, pcl::Normal, pcl::ShapeContext1980> shape_context;

  // Provide the point cloud
  shape_context.setInputCloud (cloud);
  shape_context.setInputNormals(normals);
  // Use the same KdTree 
  pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree (new pcl::search::KdTree<pcl::PointXYZ>);
  shape_context.setSearchMethod (kdtree);
  // The minimal radius is generally set to approx. 1/10 of the search radius, 
  //while the pt. density radius is generally set to 1/5
  shape_context.setRadiusSearch (radius*10);
  shape_context.setPointDensityRadius (radius*2);
  shape_context.setMinimalRadius (radius);

  // Actually compute the shape contexts
  shape_context.setIndices(ISSIdx);
  shape_context.compute (threeSCOut);
  cout<<threeSCOut.size()<<endl;
}

// void GenIn::RoPS(pcl::PointCloud<pcl::PointNormal>::ConstPtr cloud, pcl::PointIndicesPtr ISSIdx,double radius,pcl::PointCloud<pcl::Histogram <135> >& RoPSOut){
//     pcl::search::KdTree<pcl::PointNormal>::Ptr kdtree(new pcl::search::KdTree<pcl::PointNormal>);
// 	kdtree->setInputCloud(cloud);
//     //setup the Poisson mesh computation
// 	pcl::Poisson<pcl::PointNormal> pn ;
// 	// pn.setConfidence(false); //是否使用法向量的大小作为置信信息。如果false，所有法向量均归一化。
// 	// pn.setDegree(2); //设置参数degree[1,5],值越大越精细，耗时越久。
// 	// pn.setDepth(8); //树的最大深度，求解2^d x 2^d x 2^d立方体元。由于八叉树自适应采样密度，指定值仅为最大深度。
// 	// pn.setIsoDivide(8); //用于提取ISO等值面的算法的深度
// 	// pn.setManifold(false); //是否添加多边形的重心，当多边形三角化时。 设置流行标志，如果设置为true，则对多边形进行细分三角话时添加重心，设置false则不添加
// 	// pn.setOutputPolygons(false); //是否输出多边形网格（而不是三角化移动立方体的结果）
// 	// pn.setSamplesPerNode(9); //设置落入一个八叉树结点中的样本点的最小数量。无噪声，[1.0-5.0],有噪声[15.-20.]平滑
// 	// pn.setScale(1.25); //设置用于重构的立方体直径和样本边界立方体直径的比率。
// 	// pn.setSolverDivide(8); //设置求解线性方程组的Gauss-Seidel迭代方法的深度

// 	//设置搜索方法和输入点云
// 	pn.setSearchMethod(kdtree);
// 	pn.setInputCloud(cloud);
// 	//创建多变形网格，用于存储结果
// 	pcl::PolygonMesh mesh ;
// 	//执行重构
// 	pn.performReconstruction(mesh);
// 	pcl::ROPSEstimation <pcl::PointNormal, pcl::Histogram <135> > RoPS;
// 	RoPS.setSearchSurface(cloud);
// 	RoPS.setInputCloud(cloud);
// 	RoPS.setIndices(ISSIdx);
// 	RoPS.setTriangles(mesh.polygons);
// 	RoPS.setRadiusSearch(radius);
// 	RoPS.setNumberOfPartitionBins(5);
// 	RoPS.setNumberOfRotations(3);
// 	RoPS.setSupportRadius(3*radius);
// 	RoPS.compute(RoPSOut);
//     cout<<RoPSOut.size();
// }
void GenIn::multiScaleSparseFeature(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud, pcl::PointIndicesPtr PointsIdx,pcl::PointIndicesPtr ISSIdx,double inlTh,int NumScale,std::vector<std::vector<double> > &FTVector_,std::vector<std::vector<double> > &NorFTVector_,vector<Param<Col> > &atom){
    ISSExt(cloud,PointsIdx,ISSIdx, inlTh);
    std::vector<std::vector<double> > FTVector(ISSIdx->indices.size());
    pcl::PointCloud<pcl::Normal>::Ptr normals(new  pcl::PointCloud<pcl::Normal>);
    calNoramls(cloud,inlTh,normals);
    //FPFH multi scale feature
    
    // for(int iter=0;iter<NumScale;++iter){
    //     pcl::PointCloud<pcl::FPFHSignature33> fpfhOut;
    //     FPFHComp(cloud, normals,inlTh*(iter+1), ISSIdx, fpfhOut);
    //     if(fpfhOut.points.size()==0){
    //         continue;
    //     }
    //     for(int i=0;i<ISSIdx->indices.size();++i){
    //         std::vector<double>subVec;
    //         for(int j=0;j<length(fpfhOut[0].histogram);++j){
    //             subVec.push_back(fpfhOut[i].histogram[j]);
    //         }
    //         FTVector[i].insert(FTVector[i].end(),subVec.begin(),subVec.end());
    //     }
    //     fpfhOut.clear();
    // }
    
    //spin image multi scale feature
    pcl::PointCloud<pcl::Histogram<153> > spinImageOut;
     for(int iter=0;iter<NumScale;++iter){
         spinImageComp(cloud,normals,ISSIdx, inlTh*(iter+1),spinImageOut);
         if(spinImageOut.points.size()==0){
                continue;
            }
         for(int i=0;i<ISSIdx->indices.size();++i){
            std::vector<double>subVec;
            for(int j=0;j<length(spinImageOut[0].histogram);++j){
                subVec.push_back(spinImageOut[i].histogram[j]);
            }
            FTVector[i].insert(FTVector[i].end(),subVec.begin(),subVec.end());
        }
        spinImageOut.clear();
     }
    
    //RoPS multi scale feature
    // pcl::PointCloud<pcl::PointNormal>:: Ptr cloudWithNor (new pcl::PointCloud<pcl::PointNormal>);
    // pcl::concatenateFields(*cloud,*normals,*cloudWithNor);
    // pcl::PointCloud<pcl::Histogram <135> > RoPSOut;
    // for(int iter=0;iter<NumScale;++iter){
    //     RoPS(cloudWithNor, ISSIdx,inlTh*(iter+1),RoPSOut);
    //     if(RoPSOut.points.size()==0){
    //         continue;
    //     }
    //     for(int i=0;i<ISSIdx->indices.size();++i){
    //         std::vector<double>subVec;
    //         for(int j=0;j<length(RoPSOut[0].histogram);++j){
    //             subVec.push_back(RoPSOut[i].histogram[j]);
    //         }
    //         FTVector[i].insert(FTVector[i].end(),subVec.begin(),subVec.end());
    //     }
    //     RoPSOut.clear();
    // }

    //3DSC multi scale feature
    // pcl::PointCloud<pcl::ShapeContext1980> threeSCOut;
    // for(int iter=0;iter<NumScale;++iter){
    //     threeDSC(cloud,normals, ISSIdx,inlTh*(iter+1),threeSCOut);
    //     if(threeSCOut.points.size()==0){
    //         continue;
    //     }
    //     for(int i=0;i<ISSIdx->indices.size();++i){
    //         std::vector<double>subVec;
    //         for(int j=0;j<threeSCOut[0].descriptorSize();++j){
    //             subVec.push_back(threeSCOut[i].descriptor[j]);
    //         }
    //         FTVector[i].insert(FTVector[i].end(),subVec.begin(),subVec.end());
    //     }
    //     threeSCOut.clear();
    // }
    if(FTVector.size()==0){
        return ;
    }
    FTVector_=FTVector;
    normalizeFT(FTVector);
    NorFTVector_=FTVector;
    SparseCoding( FTVector,atom);

}
//compute the normals of input point cloud
void GenIn::calNoramls(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,double radius,pcl::PointCloud<pcl::Normal>::Ptr normals){
  pcl::NormalEstimationOMP<pcl::PointXYZ,pcl::Normal>nor;
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
  nor.setInputCloud(cloud);
  nor.setNumberOfThreads(10);
  nor.setSearchMethod(tree);
  nor.setRadiusSearch(3*radius);
  nor.compute(*normals);
}
void GenIn::normalizeFT(std::vector<std::vector<double> > &FTVector){
    for(int col=0;col<FTVector[0].size();++col){
        double max_;
        double min_;
        for(int row=0;row<FTVector.size();++row){
            if(row==0){
                max_=FTVector[0][col];
                min_=FTVector[0][col];
            }
            if(FTVector[row][col]>max_){
                max_=FTVector[row][col];
            }
            if(FTVector[row][col]<min_){
                min_=FTVector[row][col];
            }
        }
        if(max_!=min_){
            double range_=max_-min_;
            for(int row=0;row<FTVector.size();++row){
                FTVector[row][col]=(FTVector[row][col]-min_)/range_;
            }
        }
    }
}

void GenIn::SparseCoding(const std::vector<std::vector<double> > FTVector,vector<Param<Col> > &atom){
    std::vector<Col> word_vecs;
    
    for(int i=0;i<FTVector.size();++i){
        Col word_=Col::Zero(FTVector[i].size());
        for(int j=0;j<word_.size();++j){
            word_(j,0)=FTVector[i][j];
        }
        word_vecs.push_back(word_);
    }
    atom=Train(1,1,0.5,1e-5,word_vecs);
}

bool GenIn::cmp(const pair<int,double> a,const pair<int,double> b){
    return a.second > b.second;
}

void GenIn::featureMatching(const vector<Param<Col> > sourceF,const vector<Param<Col> > targetF,bool sort_,std::vector<std::pair<int,int> > &best_matching,std::vector<pair<int,double> > &PCCsList){
    int FDim=sourceF[0].var.rows();
    int sizeS=sourceF.size();
    int sizeT=targetF.size();
    best_matching.clear();
    PCCsList.clear();
    for(int i=0;i<sizeS;++i){
        std::pair<int,int> tem_best_matching_pair;
        std::pair<int,double> tem_best_PCCs_pair;
        double tem_best_PCCs;
        for(int j=0;j<sizeT;++j){
            double mean_x=0;double mean_y=0;
            for(int k=0;k<FDim;++k){
                mean_x +=sourceF[i].var(k,0);
                mean_y +=targetF[j].var(k,0);
            }
            mean_x=mean_x/FDim;
            mean_y=mean_y/FDim;
            double molecule=0;
            double denominator1=0;double denominator2=0;
            for(int k=0;k<FDim;++k){
                molecule +=(sourceF[i].var(k,0)-mean_x)*(targetF[j].var(k,0)-mean_y);
                denominator1 +=pow((sourceF[i].var(k,0)-mean_x),2);
                denominator2 +=pow((targetF[j].var(k,0)-mean_y),2);
            }
            double PCCs=molecule/(sqrt(denominator1)*sqrt(denominator2));
            if(j==0){
                tem_best_PCCs=PCCs;
                tem_best_matching_pair=make_pair(i,j);
                tem_best_PCCs_pair=make_pair(i,tem_best_PCCs);
            }
            else{
                if(PCCs>tem_best_PCCs){
                    tem_best_PCCs=PCCs;
                    tem_best_matching_pair=make_pair(i,j);
                    tem_best_PCCs_pair=make_pair(i,tem_best_PCCs);
                }
            }  
        }
        best_matching.push_back(tem_best_matching_pair);
        PCCsList.push_back(tem_best_PCCs_pair);
    }
    //sort the PCCs value
    if(sort_){
        sort(PCCsList.begin(),PCCsList.end(),cmp);
    }
    

}

void GenIn::mutualMatching(const vector<Param<Col> > sourceF,const vector<Param<Col> > targetF,std::pair<int,int> &best_matching_pair,std::pair<double,double> &PCCs_pair){
    std::vector<std::pair<int,int> > best_matchingST;
    std::vector<std::pair<int,int> > best_matchingTS;
    std::vector<pair<int,double> > PCCsListST;
    std::vector<pair<int,double> > PCCsListTS;
    //forward matching
    featureMatching(sourceF,targetF,true,best_matchingST,PCCsListST);
    //export matching results
    /******************************************************************************/
    ofstream matchingST("best_matchingST.txt");
    for(int i=0;i<best_matchingST.size();++i){
        matchingST<<best_matchingST[i].first<<" "<<best_matchingST[i].second<<endl;
    }
    matchingST.close();
    ofstream PCCsST("PCCsListST.txt");
    for(int i=0;i<PCCsListST.size();++i){
        PCCsST<<PCCsListST[i].first<<"  "<<PCCsListST[i].second<<endl;
    }
    PCCsST.close();
    /******************************************************************************/
    //backward matching
    featureMatching(targetF,sourceF,false,best_matchingTS,PCCsListTS);
     //export matching results
    /******************************************************************************/
    ofstream matchingTS("best_matchingTS.txt");
    for(int i=0;i<best_matchingTS.size();++i){
        matchingTS<<best_matchingTS[i].first<<" "<<best_matchingTS[i].second<<endl;
    }
    matchingTS.close();
    ofstream PCCsTS("PCCsListTS.txt");
    for(int i=0;i<PCCsListTS.size();++i){
        PCCsTS<<PCCsListTS[i].first<<"  "<<PCCsListTS[i].second<<endl;
    }
    PCCsTS.close();
     /******************************************************************************/
    for(int i=0;i<PCCsListST.size();++i){
        int FWDMatchingFirstIdx=PCCsListST[i].first;
        int FWDMatchingSecondIdx=best_matchingST[PCCsListST[i].first].second;
        int BKWMatchingSecondIDX=best_matchingTS[FWDMatchingSecondIdx].second;
        if(BKWMatchingSecondIDX==FWDMatchingFirstIdx){
            best_matching_pair=make_pair(FWDMatchingFirstIdx,FWDMatchingSecondIdx);
            int BKWMatchingFirstIDX=best_matchingTS[FWDMatchingSecondIdx].first;
            PCCs_pair=make_pair(PCCsListST[i].second,PCCsListTS[BKWMatchingFirstIDX].second);
            break;
        }
    }

}

void GenIn::getFineMatchingPoints(pcl::PointCloud<pcl::PointXYZ>::ConstPtr targetPC, pcl::PointCloud<pcl::PointXYZ>::ConstPtr sourcePC,std::vector<std::pair<int,int> > best_matching_pair_list,std::vector<std::vector<int> >pointIdxEightArea,pcl::PointCloud<pcl::PointXYZ> &targetFourPoints,pcl::PointCloud<pcl::PointXYZ> &sourceFourPoints){
    targetFourPoints.width=4;
    targetFourPoints.height=1;
    targetFourPoints.is_dense=false;
    targetFourPoints.resize(targetFourPoints.width*targetFourPoints.height);
    sourceFourPoints.width=4;
    sourceFourPoints.height=1;
    sourceFourPoints.is_dense=false;
    sourceFourPoints.resize(sourceFourPoints.width*sourceFourPoints.height);
    for(int i=0;i<4;i++){
        targetFourPoints.points[i]=targetPC->points[pointIdxEightArea[i][best_matching_pair_list[i].first]];
        sourceFourPoints.points[i]=sourcePC->points[pointIdxEightArea[i+4][best_matching_pair_list[i].second]];
    }
    
}

void GenIn::transformationCal(pcl::PointCloud<pcl::PointXYZ>::ConstPtr targetPC, pcl::PointCloud<pcl::PointXYZ>::ConstPtr sourcePC,
                       std::vector<std::pair<int,int> > best_matching_pair_list,std::vector<std::vector<int> >pointIdxEightArea,pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> ::Matrix4 &transformation){
    pcl::PointCloud<pcl::PointXYZ> targetFourPoints;
    pcl::PointCloud<pcl::PointXYZ> sourceFourPoints;
    getFineMatchingPoints(targetPC,sourcePC,best_matching_pair_list,pointIdxEightArea,targetFourPoints,sourceFourPoints);
    //export MSSF final matching points
    /******************************************************************************/
    ofstream outputT("MSSFtargetFourPoints.txt");
    for(int i=0;i<targetFourPoints.points.size();++i){
        outputT<<targetFourPoints[i].x<<"    "<< 
                targetFourPoints[i].y<<"    "<<
                targetFourPoints[i].z<<endl;

    }
    outputT.close();
    ofstream outputS("MSSFsourceFourPoints.txt");
    for(int i=0;i<sourceFourPoints.points.size();++i){
        outputS<<sourceFourPoints[i].x<<"    "<< 
                sourceFourPoints[i].y<<"    "<<
                sourceFourPoints[i].z<<endl;
    }
    /******************************************************************************/
    pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> transSVD;
    transSVD.estimateRigidTransformation(sourceFourPoints,targetFourPoints,transformation);
}


